//
// Created by vivi on 17/02/2018.
//

#pragma once

#include "core/precompiled/precompiled.h"
#include "core/engine/strandbasedhe.h"
#include "core/hairgeo/hairgeo.h"
#include "core/util/mathutil.h"
#include "core/util/eigenutil.h"
#include "core/constant/constants.h"
#include "core/collision/collisionobj.h"
#include "../accel/finitegrid.h"
#include "VPly/vply.h"
#include <fstream>
#include <initializer_list>
#include <cmath>
#include <algorithm>
#include <functional>
#include <iostream>
#include <memory>
#include <vector>
#include <numeric>
#include <iomanip>
#include <functional>
#include <unordered_set>
#include <queue>

namespace HairEngine {

	/*
	 * This hair engine is based on the "A Mass Spring Model for Hair Simulation", it implements a
	 * hair simulator with the mass spring system and provide a linear spring equations. Bending and torsion
	 * effect are modeled by bending and twist spring.
	 */
	class AndrewSelleHairEngine: public StrandBasedHairEngine {
	Public:
		typedef AndrewSelleHairEngine *Ptr;

		using StrandMassDistributionFunction = std::function<float(size_t, size_t)>;

		struct StrandMassDistribution {
			virtual float operator()(size_t index, size_t strandSize) const = 0;
		};

		struct StrandMassDistributionUniform: public StrandMassDistribution {
			float operator()(size_t index, size_t strandSize) const override {
				return 1.0f / static_cast<float>(strandSize);
			}
		};

		/*
		 * The particle mass of tips is always smaller than the root of the strand particles
		 */
		struct StrandMassDistributionNormal: public StrandMassDistribution {
			/*
			 * We assume that the mass of all particles in the strand is 1.0, except for the last few particles. The
			 * constructor uses a initializer_list<float> to indicates the last few particles' mass (usually less or
			 * equal to zero).
			 */
			StrandMassDistributionNormal(const std::initializer_list<float> & tips_) {
				tips = {};
				tipsTotal = 0.0f;

				for (const auto tip : tips_) {
					tips.push_back(tip);
					tipsTotal += tip;
				}
			}

			float operator()(size_t index, size_t strandSize) const override {
				int tipIndex = ((int)tips.size()) - 1 - ((int)strandSize - 1 - (int)index);

				float strandTotal = (strandSize >= tips.size())
				                    ? (strandSize - tips.size()) + tipsTotal
				                    : std::accumulate(tips.end() - strandSize, tips.end(), 0.0f);
				return (tipIndex < 0 ? 1.0f : tips[tipIndex]) / strandTotal;
			}

			std::vector<float> tips;
			float tipsTotal;
		};

		/*
        * The data struct that is used for AndrewSelleHairEngine initialization
        */
		struct Configuration {
			enum IntegrationMethod {
				Default,
				RungeKutta
			};

			float stiffness = 300000.0f; //Controls the stretch spring stiffness
			float bendingStiffness = 30000.0f; //Control the bending spring stiffness
			float torsionStiffness = 30000.0f; //Control the torsion spring sitffness;
			float altitudeStiffness = 2000.0f; //Controls the altitude spring (that is used to avoid degeneration) stiffness
			float contactStiffness = 90000.0f;
			float damping = 100.0f; //The damping coefficient
			float windDamping = 5.0f; //The wind damping coefficient
			float strandMass = 1.0f; //The mass of each strand is equal to (massPerLength * length-of-the-strand)
			StrandMassDistributionFunction strandMassDistributor = StrandMassDistributionUniform();


			Eigen::Vector3f externalAccleration = {0.0f, -9.8f, 0.0f}; //The total external force like gravity

			/*
			 * Since the andrew selle hair engine is totally physically-based, that we must take care of the simulation time.
			 * If the simulation time step is too large, we must divide the simulation into several pass based on the
			 * property "maxSimulationTime". Suppose the simulation timestep is 0.03f and the maxSimulationTime is 0.0075f.
			 * Then it will be divided into 4 pass for simulation, which ensure that the hair engine won't go wrong!
			 */
			float maxSimulationTime = 0.0075f;

			/*
			 * Tells which integration scheme might use for integration
			 */
			IntegrationMethod method = IntegrationMethod::Default;

			bool enableStrainLimiting = true;

			/*
			 * It tells when the deformation of strand's length exceed (1 + strainLimitingLengthTolerance), we should
			 * provide a strain limiting protection
			 */
			float strainLimitingLengthTolerance = 0.10f;

			bool enableObjectCollision = true;
			/*
			 * By default, the collision will only push the particles to the zero isocontour of the signed distance,
			 * but it sometimes looks like some part of the hair strand is inside the collision object, for we only
			 * detect the particle instead of the line segemnts. To avoid this, we instead of push it to 0 isocontour
			 * but (objectCollisionRelativeContourOffset * collisionObject.diagnoal()) isocontour.
			 */
			float objectCollisionRelativeContourOffset = 0.015f;
			float objectCollisionDegenerateFactor = 1.0f + 5e-3f; //To avoid degenerate
			float objectCollisionFriction = 6.0f;

			bool enableHairContacts = true;
			float hairContactsCreatingDistance = 3.5e-3f;
			float hairContactsBreakingDistance = 8e-3f;
			int hairContactsMaxContactPerSegment = 1;


			float hairContactsGridSize = 1e-3f;
			int hairContactsMaxGridResolution = 350;

			bool enableHairCollisions = true;
			int hairCollisionMaxCollisionPerSegment = 10;
			float hairCollisionInterestingDistance = 1e-3f;

			Configuration(float stiffness_,
			              float bendingStiffness_,
			              float torsionStiffness_,
			              float altitudeStiffness_,
			              float contactStiffness_,
			              float damping_,
			              float windDamping_,
			              float strandMass_,
			              const StrandMassDistributionFunction & strandMassDistributor_,
			              const Eigen::Vector3f & externalAccleration_,
			              float maxSimulationTime_,
			              IntegrationMethod method_,
			              bool enableStrainLimiting_,
			              float strainLimitingTolerance_,
			              bool enableObjectCollision_,
			              float objectCollisionRelativeContourOffset_,
			              bool enableHairContacts_,
						  float hairContactsCreatingDistance_,
						  float hairContactsBreakingDistance_,
						  int hairContactsMaxContactPerSegment_,
						  float hairContactsGridSize_,
						  int hairContactsMaxGridResolution_,
						  bool enableHairCollisions_,
						  int hairCollisionMaxCollisionPerSegment_,
						  float hairCollisionInterestingDistance_):
					stiffness(stiffness_),
					bendingStiffness(bendingStiffness_),
					torsionStiffness(torsionStiffness_),
					altitudeStiffness(altitudeStiffness_),
					contactStiffness(contactStiffness_),
					damping(damping_),
					windDamping(windDamping_),
					strandMass(strandMass_),
					strandMassDistributor(strandMassDistributor_),
					externalAccleration(externalAccleration_),
					maxSimulationTime(maxSimulationTime_),
					method(method_),
					enableStrainLimiting(enableStrainLimiting_),
					strainLimitingLengthTolerance(strainLimitingTolerance_),
					enableObjectCollision(enableObjectCollision_),
					objectCollisionRelativeContourOffset(objectCollisionRelativeContourOffset_),
					enableHairContacts(enableHairContacts_),
					hairContactsCreatingDistance(hairContactsCreatingDistance_),
					hairContactsBreakingDistance(hairContactsBreakingDistance_),
					hairContactsMaxContactPerSegment(hairContactsMaxContactPerSegment_),
					hairContactsGridSize(hairContactsGridSize_),
					hairContactsMaxGridResolution(hairContactsMaxGridResolution_),
					enableHairCollisions(enableHairCollisions_),
					hairCollisionMaxCollisionPerSegment(hairCollisionMaxCollisionPerSegment_),
					hairCollisionInterestingDistance(hairCollisionInterestingDistance_) {}

			Configuration() = default;
		};

		struct Strand;

		//Particle definition
		struct Particle {
			typedef Particle *Ptr;

			Eigen::Vector3f restPosition; //The input position
			Eigen::Vector3f position; //The particle's position
			Eigen::Vector3f velocity; //The particle's velocity

			Eigen::Vector3f tmpPosition; //A temp storage for position
			Eigen::Vector3f tmpVelocity; //A temp storage for velocity

			bool visible; //It is visible

			inline bool isVirtual() const { return !visible; }
			inline bool isNormal() const { return visible; }

			size_t globalIndex; //The global index of the particle
			size_t localIndex; //The local index (relative to the strand root particle index) of the particle

			Strand *strandPtr; //The strand pointer that the particle belongs to

			float mass;

			Particle(Strand *strandPtr_,
			         size_t globalIndex_,
			         size_t localIndex_,
			         bool visible_,
			         float mass_,
			         const Eigen::Vector3f & restPosition_,
			         const Eigen::Vector3f & restVelocity_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f)):

					strandPtr(strandPtr_),
					globalIndex(globalIndex_),
					localIndex(localIndex_),
					visible(visible_),
					mass(mass_),
					restPosition(restPosition_),
					position(restPosition_),
					velocity(restVelocity_) {}

			//To tell the particle if it is the root of strand
			bool isStrandRoot() const {
				return localIndex == 0;
			}

			//To tell the particle if it is the tip of strand
			bool isStrandTip() const {
				return localIndex == strandPtr->particlePtrs.size() - 1;
			}

			//Check whether they are belong to the same strand
			bool isInTheSameStrand(const Particle &other) const {
				return strandPtr == other.strandPtr;
			}

			//Store current position and velocity to the temp area
			void storeTempPositionAndVelocity() {
				tmpPosition = position;
				tmpVelocity = velocity;
			}
		};

		class HairContactSpring;

		//A segment connect two VISIBLE particles in a strand
		struct Segment {
			typedef Segment *Ptr;
			Particle::Ptr p1, p2;

			size_t globalIndex = 0; //The glboal index of the segment


			Segment(Particle::Ptr p1_, Particle::Ptr p2_):
					p1(p1_),
					p2(p2_) {}

			~Segment() {
				if (contactSprings)
					delete [] contactSprings;
			}

			/*
			 * Another point of view of segment s could be viewed as:
			 *     s = p1 + t * d, d = p2 - p1, t in [0, 1]
			 * This equation is useful for computing intersection
			 */
			Eigen::Vector3f d;
			Eigen::Vector3f dInv;

			/*
			 * We use two variables to indicate the contact springs, since a contact springs (seg1, seg2) will only sotre in
			 * seg1 if seg2.globalIndex > seg1.globalIndex. Thus, we use nContacts to indicate how many segments are stored
			 * in seg1, and nUndirectedContacts for indicaing how many contacts are related to seg1.
			 */
			size_t nContacts = 0;
			size_t nUndirectedContacts = 0;
			HairContactSpring *contactSprings = nullptr;

			/*
			 * Compute and store the d value, so that we won't do additional computation
			 * in a simulation step. So any time if we want to do the intersection staff, we must
			 * call this method first. Otherwise the result will be undefined.
			 */
			void compute() {
				d = p2->position - p1->position;
				dInv = d.cwiseInverse(); //It's okay for we having a +inf
			}

			/*
			 * Get the correct "t" value which intersects with an x slab. The return value is not bounded to [0, 1]
			 * if the actual intersection point is outside the segment
			 */
			float xSlabIntersection(float x) const {
				return (x - p1->position.x()) * dInv.x();
			}

			float ySlabIntersection(float y) const {
				return (y - p1->position.y()) * dInv.y();
			}

			float zSlabIntersection(float z) const {
				return (z - p1->position.z()) * dInv.z();
			}

			size_t localIndex() const {
				return p1->localIndex;
			}

			size_t strandIndex() const {
				return p1->strandPtr->index;
			}

			HairContactSpring *contactSpringBegin() {
				return contactSprings;
			}

			const HairContactSpring *contactSpringBegin() const {
				return contactSprings;
			}

			HairContactSpring *contactSpringEnd() {
				return contactSprings + nContacts;
			}

			const HairContactSpring *contactSpringEnd() const {
				return contactSprings + nContacts;
			}

			Eigen::Vector3f center() {
				return (p1->position + p2->position) / 2.0f;
			}

			Eigen::Vector3f prevCenter() {
				return (p1->tmpPosition + p2->tmpPosition) / 2.0f;
			}
		};

		/*
		 * Particles in the strands are connected with mass spring, mass spring provide spring forces that make
		 * the particle stick together and maintain the volume of a strand. The MassSpring is the base class, currently
		 * we support two derived class, ParticleMassSpring is the normal mass spring which connects two particles, the
		 * TetrahedralMassSpring is the "altitude spring" described in Andrew Selle's "A Mass Spring for Hair Simulation"
		 * which is used to avoid degeneration for the tetrahedral. Each MassSpring subclass should implement the "compute"
		 * the method and fill the correct value to the "i"(Eigen::Vector3f), "d(Eigen::Vector3f)", "dm(Eigen::Vector3f)".
		 */

		struct Spring {
			float k; //Represent stiffness

			mutable float l0; //Represent the rest length

			Spring(float k_, float l0_):
					k(k_),
					l0(l0_) {}

			mutable float l; //Current spring length

			/*
			 * Current spring normalized direction.
			 * For particle/particle, d points from p1 to p2
			 * For altitude, the d points from p1 to other edge/face.
			 */
			mutable Eigen::Vector3f d;

			/*
			 * The impluse. For particle/particle mass spring, it represents the force impulse that appied to p1
			 * which equals to k * d * (l - l0).
			 * For particle/particle, it means the force that p2 applies to p1
			 */
			mutable Eigen::Vector3f f;

			/*
			 * The direction matrix, which equals to d.transpose() * d, all the particle has the same
			 * direction matrix. A direction matrix dm generated by d multiply a vector v means that
			 * the projection vector of v on d.
			 */
			mutable Eigen::Matrix3f dm;

			/*
			 * When the simulator wants the newest value for d, i, l and dm after position change in the particles,
			 * it should first call "compute" and use those values freely. Since all the spring has the same pattern for
			 * computing the l, i and dm if we know the direction d. We implement it in the base class so that the
			 * derived class only needs to provide the method to get the correct direction.
			 */
			void compute() const {
				d = computeD();
				l = d.norm();

				if (l != 0) {
					d /= l;
					f = (k * (l - l0)) * d;
				}
				else {
					f = Eigen::Vector3f::Zero();
				}


				dm(0, 0) = d(0) * d(0);
				dm(1, 1) = d(1) * d(1);
				dm(2, 2) = d(2) * d(2);

				dm(0, 1) = dm(1, 0) = d(0) * d(1);
				dm(0, 2) = dm(2, 0) = d(0) * d(2);
				dm(1, 2) = dm(2, 1) = d(1) * d(2);
			}

		Protected:

			/*
			 * The derived class should implement this method to compute the d (WITHOUT NORMALIZED), called by the
			 * "compute" method.
			 */
			virtual Eigen::Vector3f computeD() const = 0;

			/*
			 * Sometimes we cannot get the l0 before initialization, we provide a method to set the l0 and recompute the
			 * kDivL0
			 */
			void setL0(float l0_) const {
				l0 = l0_;
			}
		};

		struct ParticleToParticleSpring: public Spring {
			enum Type {
				Stretch, VirtualStretch, Bending, Torsion
			};

			Particle::Ptr  p1, p2; //p1.localIndex < p2.localIndex, which means p1 is more closer to the root
			Type type;

			ParticleToParticleSpring(const Particle::Ptr p1_, const Particle::Ptr p2_, float k_, Type type_):
					Spring(k_, (p2_->position - p1_->position).norm()),
					p1(p1_),
					p2(p2_),
					type(type_) {
				if (p1->localIndex > p2->localIndex) {
					p1 = p2_;
					p2 = p1_;
				}
			}

			virtual Eigen::Vector3f computeD() const { return p2->position - p1->position; }

			/*
			 * Get the average velocity of the segments
			 */
			Eigen::Vector3f getVelocity() const {
				return 0.5f * (p1->velocity + p2->velocity);
			}
		};

		struct AltitudeSpring: public Spring {
			const Particle::Ptr p1, p2, p3, p4;

			/*
			 * A temporary storage to store some caculation variables while computeD. In a tetrahedral, we have 3
			 * edge/edge springs and 4 point/face spring, so that we use a array of size 7(3 + 4) to store them.
			 * The order is:
			 *     0. Edge/Edge: {p1, p2} --> {p3, p4}
			 *     1. Edge/Edge: {p1, p3} --> {p2, p4}
			 *     2. Edge/Edge: {p1, p4} --> {p2, p3}
			 *     3. Point/Face: p1 --> {p2, p3, p4}
			 *     4. Point/Face: p2 --> {p1, p3, p4}
			 *     5. Point/Face: p3 --> {p1, p2, p4}
			 *     6. Point/Face: p4 --> {p1, p2, p3}
			 * The l0s variables store the rest length of each tetrahedral elements, normals is used for storing the
			 * current normals, and vs means a point/point vector which points from p1 to another edge/face vertex
			 * (since we want the i, and d are pointed from p1), for example, for a Point/Face p4 --> {p1, p2, p3},
			 * the v is p1 --> p4.
			 */
			mutable float l0s[7] = {0};
			mutable Eigen::Vector3f normals[7];

			mutable size_t selectionIndex = -1;

			/*
			 * A vector point from p1 to another edge/edge or point/face points. It depends on which type it is.
			 */
			mutable Eigen::Vector3f vs[7];

			AltitudeSpring(Particle::Ptr p1_,
			               Particle::Ptr p2_,
			               Particle::Ptr p3_,
			               Particle::Ptr p4_,
			               float k_):
					Spring(k_, 0.0f),
					p1(p1_), p2(p2_), p3(p3_), p4(p4_) {

				getAltitudeSpringIndex();

				for (size_t i = 0; i < 7; ++i) {
					normals[i].normalize();
					l0s[i] = MathUtility::project(vs[i], normals[i]).norm();
				}
			}

			const Eigen::Vector3f & pos1() const { return p1->position; }
			const Eigen::Vector3f & pos2() const { return p2->position; }
			const Eigen::Vector3f & pos3() const { return p3->position; }
			const Eigen::Vector3f & pos4() const { return p4->position; }

			/*
			 * A helper function for getting the largest cross norm of the 7 edge/edge or point/face.
			 * For and edge-edge spring, the cross product is the two line segment vectors; For point/face spring,
			 * the cross product is the two edge vectors of the face. Its worth for noting that this function will
			 * fill the normals and vs vector.
			 */
			size_t getAltitudeSpringIndex() const {

				Eigen::Vector3f
					d12 = p2->position - p1->position,
					d13 = p3->position - p1->position,
					d14 = p4->position - p1->position,
					d23 = p3->position - p2->position,
					d24 = p4->position - p2->position,
					d34 = p4->position - p3->position;

				vs[0] = d13;
				vs[1] = d12;
				vs[2] = d12;

				vs[3] = d12;
				vs[4] = d12;
				vs[5] = d13;
				vs[6] = d14;

				normals[0] = d12.cross(d34);
				normals[1] = d13.cross(d24);
				normals[2] = d14.cross(d23);

				normals[3] = d23.cross(d24);
				normals[4] = d13.cross(d14);
				normals[5] = d12.cross(d14);
				normals[6] = d12.cross(d23);

				size_t largestIndex = 0;
				float largestSquaredNormal = normals[0].squaredNorm();

				for (size_t i = 1; i < 7; ++i) {
					float squaredNormal = normals[i].squaredNorm();
					if (squaredNormal > largestSquaredNormal) {
						largestIndex = i;
						largestSquaredNormal = squaredNormal;
					}
				}

				//Set the edgeParticle
				selectionIndex = largestIndex;

				return selectionIndex;
			};

			Eigen::Vector3f computeD() const override {

				size_t i = getAltitudeSpringIndex();

				setL0(l0s[i]);

				normals[i].normalize();
				Eigen::Vector3f vProject = MathUtility::project(vs[i], normals[i]);

				//Make the direction forward to p1 -> another point
				if (vProject.dot(vs[i]) < 0)
					vProject = -vProject;

				return vProject;
			}
		};

		/*
		 * A contact for two segments, it derived from the Spring interface
		 */
		struct HairContactSpring: Spring {
			typedef HairContactSpring *Ptr;

			Segment::Ptr l1, l2;
			float t1, t2; //The attached point for creation

			struct Hasher {
				Hasher() = default;
				size_t operator()(const HairContactSpring & pair) const {
					std::hash<Segment::Ptr> ptrHasher;
					return ptrHasher(pair.l1) ^ ptrHasher(pair.l2);
				}
			};

			HairContactSpring(const Segment::Ptr & l1_, const Segment::Ptr & l2_, float t1_, float t2_, float l0_, float k_):
					Spring(k_, l0_),
					l1(l1_),
					l2(l2_),
					t1(t1_),
					t2(t2_) {}

			bool operator==(const HairContactSpring & rhs) const {
				return (l1 == rhs.l1 && l2 == rhs.l2) || (l1 == rhs.l2 && l2 == rhs.l1);
			}

			Eigen::Vector3f pos1() const {
				return MathUtility::lerp(l1->p1->position, l1->p2->position, t1);
			}

			Eigen::Vector3f pos2() const {
				return MathUtility::lerp(l2->p1->position, l2->p2->position, t2);
			}

			//We assume that both l1 and l2's d value is correctly computed
			Eigen::Vector3f computeD() const {
				return pos2() - pos1();
			}
		};

		//Strand definition
		struct Strand {
			typedef Strand *Ptr;

			AndrewSelleHairEngine::Ptr enginePtr;
			size_t index; //The index in the engine
			size_t particleIndex; //The index for the hair root in the engine (i.e the globalIndex of the hairRoot)

			float mass; //The total mass of the strand

			//Particles
			std::vector<Particle::Ptr> particlePtrs; //All particles in the strand
			std::vector<Particle::Ptr> visibleParticlePtrs; //Visible particles in the strand
			Particle::Ptr strandRootParticle = nullptr, strandTipParticle = nullptr; //Root and tip in the strand

			//Segments
			std::vector<Segment> segments;

			//Springs
			std::vector<ParticleToParticleSpring> normalSprings;
			std::vector<AltitudeSpring> altitudeSprings;

			Strand(const AndrewSelleHairEngine::Ptr enginePtr_,
			       size_t index_, size_t particleIndex_,
			       float mass_,
			       const StrandMassDistributionFunction & massDistributor_,
			       const HairGeometry::StrandParticles & s):
					enginePtr(enginePtr_),
					index(index_),
					particleIndex(particleIndex_),
					mass(mass_) {

				size_t localIndexCounter = 0;

				/*
				 * In order to make sure the additional (virtual) particle should be no-colinear, we store
				 * the previous virtual particle projection vector. Each time we insert a new virtual particle,
				 * we rotate the projection vector to 90 degree so that it will not collapse.
				 */
				Eigen::Vector3f prevVirtualParticleProjectionVector;
				bool isPrevVirtualParticleProjectionVectorInitialized = false;

				//Add particles and virtual particles
				for (size_t i = 0; i < s.size; ++i) {

					//Create a particle and add it
					float particleMass = mass * massDistributor_(i, s.size);
					Particle::Ptr particle = createParticle(s.particles[i], true, particleMass, localIndexCounter);

					//If we have a segment s --> s + 1
					if (i + 1 < s.size) {
						const auto & d = s.particles[i + 1] - s.particles[i];

						/*
						 * Now compare the segment (i --> i + 1) to (i - 1 --> i) and (i + 1 --> i + 2) to check
						 * if it is colinear or not. Is so, create a virtual particle.
						 */
						if ((i >= 1 && MathUtility::isColinear(s.particles[i] - s.particles[i - 1], d, 0.0872665f)) ||
							(i + 2 < s.size && MathUtility::isColinear(d, s.particles[i + 2] - s.particles[i + 1], 0.0872665f))) {

							if (!isPrevVirtualParticleProjectionVectorInitialized) {
								prevVirtualParticleProjectionVector = MathUtility::makeOrthogonalVector(d, false);
								isPrevVirtualParticleProjectionVectorInitialized = true;
							}
							else {
								prevVirtualParticleProjectionVector = d.cross(prevVirtualParticleProjectionVector);
							}

							//Make the length equal to sqrt(3) / 2 * d.norm()
							prevVirtualParticleProjectionVector *= (0.866025f * d.norm() / prevVirtualParticleProjectionVector.norm());

							createParticle(s.particles[i] + 0.5f * d + prevVirtualParticleProjectionVector, false, particleMass, localIndexCounter);
							//When we create a virtual particle, we distribute half of the mass of the previous particle to it
							//particle->mass *= 0.5f;
						}
					}
				}

				//Assign the strand root particle and strand tip particle
				strandRootParticle = particlePtrs.front();
				strandTipParticle = particlePtrs.back();


				//Add segments
				for (size_t i = 1; i < visibleParticlePtrs.size(); ++i)
					createSegment(visibleParticlePtrs[i - 1], visibleParticlePtrs[i]);

				//Add Springs
				for (size_t i = 0; i < particlePtrs.size(); ++i) {
					Particle::Ptr
						p = particlePtrs[i],
						p1 = i + 1 < particlePtrs.size() ? particlePtrs[i + 1] : nullptr,
						p2 = i + 2 < particlePtrs.size() ? particlePtrs[i + 2] : nullptr,
						p3 = i + 3 < particlePtrs.size() ? particlePtrs[i + 3] : nullptr;

					if (p->isNormal()) {
						//Stretch spring is connected to the next normal particle
						if (p1 && p1->isNormal())
							createNormalSpring(p, p1, ParticleToParticleSpring::Type::Stretch);
						else if (p2 && p2->isNormal())
							createNormalSpring(p, p2, ParticleToParticleSpring::Type::Stretch);

						//Virtual stretch spring is connected to the next particle is the particle is virtual
						if (p1 && p1->isVirtual())
							createNormalSpring(p, p1, ParticleToParticleSpring::Type::VirtualStretch);

						/*
						 * Bending spring is connected to the next 2 normal particle, we ignore bending spring
						 * in the colinear particle groups.
						 */
						if (p2 && p1->isNormal() && p2->isNormal())
							createNormalSpring(p, p2, ParticleToParticleSpring::Type::Bending);
						else if (p3 && p1->isVirtual() && p2->isNormal() && p3->isNormal())
							createNormalSpring(p, p3, ParticleToParticleSpring::Type::Bending);

						/*
						 * Stretch spring usually connected to the next 3 particle, unless we meet p1 is normal and
						 * p2 is virtual case
						 */
						if (p2 && p1->isNormal() && p2->isVirtual())
							createNormalSpring(p, p2, ParticleToParticleSpring::Type::Torsion);
						else if (p3)
							createNormalSpring(p, p3, ParticleToParticleSpring::Type::Torsion);
					}
					else {
						//Virtual stretch is always made to the next particle
						if (p1)
							createNormalSpring(p, p1, ParticleToParticleSpring::Type::VirtualStretch);

						//Bending is connected to the next virtual particle
						if (p2 && p2->isVirtual())
							createNormalSpring(p, p2, ParticleToParticleSpring::Type::Bending);

						/*
						 * Torsion is always connected to the next 3 particle, unless we meet the p1 is normal and
						 * p2 is normal case (which means is the end virtual particles in the colinear group)
						 */
						if (p2 && p1->isNormal() && p2->isNormal())
							createNormalSpring(p, p2, ParticleToParticleSpring::Type::Torsion);
						else if (p3)
							createNormalSpring(p, p3, ParticleToParticleSpring::Type::Torsion);
					}
				}
			}

			virtual ~Strand() {
				//Since visibleParticlePtrs points to a subset of particlePtrs, we don't need to destory it
				for (auto & particlePtr : particlePtrs)
					delete particlePtr;
			}

			size_t getParticleSize() const { return particlePtrs.size(); }
			size_t getNormalParticleSize() const { return visibleParticlePtrs.size(); }

		Private:
			Particle::Ptr createParticle(const Eigen::Vector3f & pos, bool visible, float mass, size_t & localIndexCounter) {
				auto ret = new Particle(this, particleIndex + localIndexCounter, localIndexCounter, visible, mass, pos);
				++localIndexCounter;

				particlePtrs.push_back(ret);
				if (ret->visible)
					visibleParticlePtrs.push_back(ret);

				return ret;
			};

			void createSegment(Particle::Ptr p1, Particle::Ptr p2) {
				segments.emplace_back(p1, p2);
			}

			void createNormalSpring(Particle::Ptr p1, Particle::Ptr p2, ParticleToParticleSpring::Type type) {
				Assert(p1->localIndex != p2->localIndex);

				if (p1->localIndex > p2->localIndex)
					std::swap(p1, p2);

				normalSprings.emplace_back(p1, p2, enginePtr->conf.stiffness, type);
			}

			void createAltitudeSpring(Particle::Ptr p1, Particle::Ptr p2, Particle::Ptr p3, Particle::Ptr p4) {
				Particle::Ptr p[4] = { p1, p2, p3, p4 };

				std::sort(p, p + 4, [](const Particle::Ptr & p1, const Particle::Ptr & p2) -> bool {
					return p1->localIndex < p2->localIndex;
				});

				altitudeSprings.emplace_back(p[0], p[1], p[2], p[3], enginePtr->conf.altitudeStiffness);
			}
		};

		//Information for collision
		struct CollisionInfo {

			struct Hasher {
				size_t operator()(const CollisionInfo & info) const {
					std::hash<Segment::Ptr> hasher;
					return hasher(info.s1) ^ hasher(info.s2);
				}

				size_t operator()(const std::pair<Segment::Ptr, Segment::Ptr> & pair) const {
					std::hash<Segment::Ptr> hasher;
					return hasher(pair.first) ^ hasher(pair.second);
				}
			};

			Segment::Ptr s1, s2; //Two collision
			float t1, t2;

			Eigen::Vector3f n; //Original normal
			float l0; //Original length

			CollisionInfo(Segment::Ptr s1_,
			              Segment::Ptr s2_,
			              float t1_,
			              float t2_,
			              const Eigen::Vector3f & n_,
			              float l0_):
					s1(s1_), s2(s2_),
					t1(t1_), t2(t2_),
					n(n_), l0(l0_) {}

			Eigen::Vector3f d() const {
				Eigen::Vector3f s2p = MathUtility::lerp(s2->p1->position, s2->p2->position, t2);
				Eigen::Vector3f s1p = MathUtility::lerp(s1->p1->position, s1->p2->position, t1);
				return (s2p - s1p);
			}

			Eigen::Vector3f estimatedD(float t) const {
				Eigen::Vector3f s2p = MathUtility::lerp((s2->p1->position + t * s2->p1->velocity).eval(), (s2->p2->position + t * s2->p2->velocity).eval(), t2);
				Eigen::Vector3f s1p = MathUtility::lerp((s1->p1->position + t * s1->p1->velocity).eval(), (s1->p2->position + t * s1->p2->velocity).eval(), t1);
				return s2p - s1p;
			}
		};

	Public:

		AndrewSelleHairEngine(const StrandBasedHairGeometry & geo,
		                      const Configuration & conf_,
		                      std::shared_ptr<CollisionObject> aggregate_ = nullptr):
				conf(conf_),
				aggregate(aggregate_),
				aggregateInitialTransform(aggregate_ ? aggregate_->getLocalToWorldTransform() : Eigen::Affine3f::Identity()),
				grid(conf_.hairContactsGridSize * Eigen::Vector3f::Ones(), conf_.hairContactsMaxGridResolution * Eigen::Vector3i::Ones()) {

			//We use a very large float value to force the rotation velocity to 0
			updateCollisionTransform(aggregateInitialTransform, 1e30f, false);

			strands.reserve(geo.nstrand);
			for (size_t i = 0; i < geo.nstrand; ++i) {
				strands.emplace_back(this, i, particlePtrs.size(), conf.strandMass, conf.strandMassDistributor, geo.strandStructure(i));

				auto &strand = strands.back();

				for (auto &particlePtr : strand.particlePtrs) {
					particlePtrs.push_back(particlePtr);
					if (particlePtr->isNormal())
						normalParticlePtrs.push_back(particlePtr);
					else
						virtualParticlePtrs.push_back(particlePtr);
				}
			}

			size_t segmentGlobalCounter = 0;
			for (auto & strand : strands) {
				for (auto & segment : strand.segments) {
					segment.globalIndex = segmentGlobalCounter++;
					segmentPtrs.push_back(&segment);
				}
			}

			segmentCheck = std::vector<Segment::Ptr>(segmentPtrs.size());

			particleImpulses = std::vector<Eigen::Vector3f>(particlePtrs.size(), Eigen::Vector3f::Zero());

			//We assume the number of contacts is fixed
			for (auto & segmentPtr : segmentPtrs) {
				segmentPtr->contactSprings = std::allocator<HairContactSpring>().allocate(
						(size_t)(conf.hairContactsMaxContactPerSegment));
			}
		}

		void simulate(const HairEngineSimulationData & data) override {
			if (conf.maxSimulationTime >= data.t)
				_simulate(data);
			else {
				HairEngineSimulationData dataForPass = data;
				dataForPass.transform = data.previousTransform;

				for (float time = conf.maxSimulationTime; time <= data.t + 1e-8f;) {
					float lerpPoint = time / data.t;
					dataForPass.previousTransform = dataForPass.transform;
					dataForPass.transform = EigenUtility::lerp(lerpPoint, data.previousTransform, data.transform);
					dataForPass.t = conf.maxSimulationTime;

					time += conf.maxSimulationTime;

					_simulate(dataForPass);
				}
			}
		}

		/*
		 * Sometimes the hair style may not be fit for the body, some hair may be inside the body. We introduce a
		 * method to do some preprocessing the the hair style to make all the hair outside the body.
		 */
		void reGeometry(size_t nIteration, float iterationTime, float relativeContour = 0.0f, bool updateStrandRootPosition = false) {
			//If no any collision object, we just return
			if (aggregate == nullptr)
				return;

			Configuration confBackup = this->conf;

			//Set the external force to 0.0
			this->conf.externalAccleration = Eigen::Vector3f::Zero();
			this->conf.enableObjectCollision = true;
			this->conf.objectCollisionRelativeContourOffset = relativeContour;

			//Set the timestep for iteration
			this->conf.maxSimulationTime = std::numeric_limits<float>::infinity();

			//Use identity transform
			HairEngineSimulationData data = HairEngineSimulationData(Eigen::Affine3f::Identity(), Eigen::Affine3f::Identity(), iterationTime);

			for (size_t i = 0; i < nIteration; ++i) {
				solveStrandDynamics(data, true);
				solveObjectCollision(data, updateStrandRootPosition);
			}

			//Restore configutation
			this->conf = confBackup;
		}

		/*
		 * Update the collision transform, if "fromInitial" parameter is true, the transform of the collision object should be
		 * transform * initalTransform. Ohterwise, collision object's transform will be equal to transform. Time is used
		 * for cauclating the collision object's velocity
		 */
		void updateCollisionTransform(const Eigen::Affine3f & transform, const float t, bool fromInitial) {
			if (!aggregate)
				return;

			if (fromInitial)
				aggregate->updateRigidTransform(transform * aggregateInitialTransform, t);
			else
				aggregate->updateRigidTransform(transform, t);
		}

		//Delegate for implementation
		inline virtual size_t getParticleCount() const override {
			return normalParticlePtrs.size();
		}

		inline virtual size_t getStrandCount() const override {
			return strands.size();
		}

		inline virtual size_t getParticleCountOnStrand(size_t strandNumber) const override {
			/*
			 * We are not using the "particlePtrs" but use the "visibleParticlePtrs" because the virtual particle are only
			 * use for simulation, they are not "visible" to the outer part.
			 */
			return strands[strandNumber].visibleParticlePtrs.size();
		}

		inline virtual const Eigen::Vector3f & getParticlePosition(size_t strandNumber, size_t localNumber) const override {
			return strands[strandNumber].visibleParticlePtrs[localNumber]->position;
		}

		/*
		 * Use to test the altitude spring, we will force the particle's position co-plane, and test the ability of
		 * the alitutude spring
		 */
		void debugTestDegenerate() {
			for (auto & strand : strands) {
				const Eigen::Vector3f rootPos = strand.strandRootParticle->position;

				Eigen::Vector3f planeNormal = Eigen::Vector3f::Random();
				planeNormal(1) = 0.0f; //We make the plane in y axis
				planeNormal.normalize();

				for (size_t i = 1; i < strand.getParticleSize(); ++i) {
					auto & particlePtr = strand.particlePtrs[i];
					const Eigen::Vector3f rootPosToParticlePos = particlePtr->position - rootPos;

					float direction = rootPosToParticlePos.dot(planeNormal) > 0 ? 1.0f : -1.0f;

					particlePtr->position = rootPos + (rootPosToParticlePos - rootPosToParticlePos.dot(planeNormal) * planeNormal);
					/*
					 * In order to make them no real coplane (otherwise the d will be 0 vector), we add a small randomness
					 * to push then forward or backward along the plane normal
					 */
					particlePtr->position +=
							(particlePtr->position - rootPos).norm() * direction * Eigen::internal::random(0.0f, 0.003f) * planeNormal;
					particlePtr->velocity = Eigen::Vector3f::Zero();
				}
			}
		}

		bool writeVPlyToFile(const std::string &filePath, const VPlyOptions options) {
			std::fstream vplyFile(filePath, std::ios::out);
			if (!vplyFile.is_open()) {
				printf("Error while opening \"%s\"\n", filePath.c_str());
				return false;
			}

			Assert(vplyFile);
			vplyFile.setf(std::ios::showpoint);

			for (const auto & strand : strands) {
				for (const auto & particlePtr : strand.particlePtrs) {
					if (((options & VPlyOptions::NormalParticles) && particlePtr->isNormal())
					    || ((options & VPlyOptions::VirtualParticles) && particlePtr->isVirtual())) {

						VPly::writePoint(
								vplyFile,
								EigenUtility::toVPlyVector3f(particlePtr->position),
								VPly::VPlyIntAttr("type", particlePtr->isNormal() ? VPlyOptions::NormalParticles : VPlyOptions::VirtualParticles),
								VPly::VPlyVector3fAttr("velocity", EigenUtility::toVPlyVector3f(particlePtr->velocity)),
								VPly::VPlyVector3fAttr("tmpvelocity", EigenUtility::toVPlyVector3f(particlePtr->tmpVelocity)),
								VPly::VPlyVector3fAttr("tmpposition", EigenUtility::toVPlyVector3f(particlePtr->tmpPosition)),
								VPly::VPlyIntAttr("strandid", (int)strand.index),
								VPly::VPlyIntAttr("globalid", (int)particlePtr->globalIndex),
								VPly::VPlyIntAttr("localid", (int)particlePtr->localIndex),
								VPly::VPlyFloatAttr("mass", particlePtr->mass)
						);
						Assert(vplyFile);
					}
				}

				for (const auto & spring : strand.normalSprings) {
					bool shouldWriteSpring = false;
					size_t writeType = 0;

					if ((options & VPlyOptions::StretchSprings)
					    && spring.type == ParticleToParticleSpring::Type::Stretch) {
						shouldWriteSpring = true;
						writeType = VPlyOptions::StretchSprings;
					}
					else if ((options & VPlyOptions::BendingSprings)
					         && spring.type == ParticleToParticleSpring::Type::Bending) {
						shouldWriteSpring = true;
						writeType = VPlyOptions::BendingSprings;
					}
					else if ((options & VPlyOptions::TorsionSprings)
					         && spring.type == ParticleToParticleSpring::Type::Torsion) {
						shouldWriteSpring = true;
						writeType = VPlyOptions::TorsionSprings;
					}
					else if ((options & VPlyOptions::VirtualStretchSprings)
					         && spring.type == ParticleToParticleSpring::Type::VirtualStretch) {
						shouldWriteSpring = true;
						writeType = VPlyOptions::VirtualStretchSprings;
					}

					if (!shouldWriteSpring)
						continue;

					VPly::writeLine(
							vplyFile,
							EigenUtility::toVPlyVector3f(spring.p1->position),
							EigenUtility::toVPlyVector3f(spring.p2->position),
							VPly::VPlyIntAttr("type", (int32_t)writeType),
							VPly::VPlyVector3fAttr("velocity", EigenUtility::toVPlyVector3f(spring.getVelocity()))
					);

					Assert(vplyFile);
				}


				if (options & VPlyOptions::AltitudeSprings) {
					for (const auto & altitudeSpring : strand.altitudeSprings) {
						size_t springIndex = altitudeSpring.getAltitudeSpringIndex();
						const Eigen::Vector3f & p1 = altitudeSpring.p1->position;
						const Eigen::Vector3f & p2 = altitudeSpring.p2->position;
						const Eigen::Vector3f & p3 = altitudeSpring.p3->position;
						const Eigen::Vector3f & p4 = altitudeSpring.p4->position;

						Eigen::Vector3f point1, point2; //Two write point
						switch (springIndex) {
							case 0:
								point1 = EigenUtility::midPoint(p1, p2);
								point2 = EigenUtility::midPoint(p3, p4);
								break;
							case 1:
								point1 = EigenUtility::midPoint(p1, p3);
								point2 = EigenUtility::midPoint(p2, p4);
								break;
							case 2:
								point1 = EigenUtility::midPoint(p1, p4);
								point2 = EigenUtility::midPoint(p2, p3);
								break;
							case 3:
								point1 = p1;
								point2 = EigenUtility::triangleCenter(p2, p3, p4);
								break;
							case 4:
								point1 = p2;
								point2 = EigenUtility::triangleCenter(p1, p3, p4);
								break;
							case 5:
								point1 = p3;
								point2 = EigenUtility::triangleCenter(p1, p2, p4);
								break;
							default:
								point1 = p4;
								point2 = EigenUtility::triangleCenter(p1, p2, p3);
								break;
						}

						VPly::writeLine(
								vplyFile,
								EigenUtility::toVPlyVector3f(point1),
								EigenUtility::toVPlyVector3f(point2),
								VPly::VPlyIntAttr("type", VPlyOptions::AltitudeSprings)
						);
					}
				}
			}

			//Output the collision object
			if (options & VPlyOptions::CollisionObject_)
				if (aggregate)
					vplyFile << (*aggregate);

			//Output the grid map
			if (options & VPlyOptions::HairContactsGrid) {
				for (SegmentGrid::GridIndex index = 0; index < grid.getOffsetGlobal(); ++index) {
					if (!grid.getElementsInCell(index).empty()) {
						auto aabb = grid.getBound(index);
						VPly::writeAABB(
								vplyFile,
								EigenUtility::toVPlyVector3f(aabb.min()),
								EigenUtility::toVPlyVector3f(aabb.max()),
								VPly::VPlyIntAttr("type", VPlyOptions::HairContactsGrid)
						);
					}
				}
			}

			if (options & VPlyOptions::HairContactsSpring) {
				for (const auto & segmentPtr : segmentPtrs) {
					for (auto l = segmentPtr->contactSpringBegin(); l != segmentPtr->contactSpringEnd(); ++l) {
						VPly::writeLine(
								vplyFile,
								EigenUtility::toVPlyVector3f(l->pos1()),
								EigenUtility::toVPlyVector3f(l->pos2()),
								VPly::VPlyIntAttr("type", VPlyOptions::HairContactsSpring),
								VPly::VPlyIntAttr("state", l->l <= l->l0 ? 0 : 1)
						);
					}
				}
			}

			if (options & VPlyOptions::HairCollisionsSpring) {
				for (const auto & info : collisionInfos) {
					VPly::writeLine(
							vplyFile,
							EigenUtility::toVPlyVector3f(MathUtility::lerp(info.s1->p1->position, info.s1->p2->position, info.t1)),
							EigenUtility::toVPlyVector3f(MathUtility::lerp(info.s2->p1->position, info.s2->p2->position, info.t2)),
							VPly::VPlyIntAttr("type", VPlyOptions::HairCollisionsSpring)
							);
				}
			}

			return true;
		}

		virtual ~AndrewSelleHairEngine() = default;

	Public:

		Configuration conf;

	Protected:

		std::vector<Strand> strands; //Represents the strands in the hair engine
		std::vector<Segment::Ptr> segmentPtrs; //All the segments in the hair engine
		std::vector<Particle::Ptr> particlePtrs; //All the particles in the hair engine
		std::vector<Particle::Ptr> normalParticlePtrs; //All the visible particles in the hair engine
		std::vector<Particle::Ptr> virtualParticlePtrs; //All the invisible particles

		std::vector<Segment::Ptr> segmentCheck; //Use in hair contacts and collision to avoid duplicate checking

		std::vector<Eigen::Vector3f> particleImpulses; //Use for storing impluse for each particles, so that we could parallel solve each strand dynamics

		std::vector<CollisionInfo> collisionInfos; //Use for checking whether the collision happend

		std::shared_ptr<CollisionObject> aggregate;
		Eigen::Affine3f aggregateInitialTransform;

		using SegmentGrid = FiniteGrid<Segment::Ptr>;
		SegmentGrid grid;

		struct SegmentGridRecord {
			Segment::Ptr l; //The segment is inserted
			SegmentGrid::GridIndex index; //The grid index for the segment
			int indicator; //The indicator for insertion (0 for initialization, 1(-1) for x, 2(-2) for y, 3(-3) for z)

			SegmentGridRecord(Segment::Ptr l_, const SegmentGrid::GridIndex & index_, int indicator_):
					l(l_),
					index(index_),
					indicator(indicator_) {}
		};

		/*
		 * The _simulate is used to simulate small timestep, the data is not equal to
		 * the data in "simulate" method if the pass is not 1. The transform and previous transform and the timestep
		 * t has been modified.
		 */
		void _simulate(const HairEngineSimulationData & data) {
			switch (conf.method) {
				case Configuration::IntegrationMethod::Default:
					integrationDefault(data);
					break;
				default:
					integrationRungeKutta(data);
					break;
			}
		}

		void integrationDefault(const HairEngineSimulationData & data) {
			restorePreviousState();
			solveHairContacts(data);
			solveStrandDynamics(data, false);
			solveStrainLimiting(data);
			solveObjectCollision(data);
		}

		void integrationRungeKutta(const HairEngineSimulationData & data) {
			//FIXME: Long time no update
			const auto halfTransform = EigenUtility::lerp(0.5, data.previousTransform, data.transform);
			const auto & t = data.t;

			const auto halfData1 = HairEngineSimulationData(halfTransform, data.previousTransform, t * 0.5f);
			const auto halfData2 = HairEngineSimulationData(data.transform, halfTransform, t * 0.5f);

			for (auto & particlePtr : particlePtrs)
				particlePtr->storeTempPositionAndVelocity();

			//We update the position manually
			solveStrandDynamics(halfData1, false);

			//Modify velocity with strain limiting
			solveStrainLimiting(halfData1);

			//Body collisions modify position and velocity
			solveHairContacts(halfData1);

			solveObjectCollision(halfData1);

			solveStrandDynamics(halfData1, false);

			for (auto & particlePtr : particlePtrs) {
				particlePtr->position = 2.0f * particlePtr->position - particlePtr->tmpPosition;
				particlePtr->velocity = 2.0f * particlePtr->velocity - particlePtr->tmpVelocity;
			}
		}

		void restorePreviousState() {
			for (auto particlePtr : particlePtrs) {
				particlePtr->storeTempPositionAndVelocity();
			}
		}

		/*
		 * Solve the strand dynamics.
		 */
		void solveStrandDynamics(const HairEngineSimulationData & data, bool shouldUpdatePosition = true) {
			const auto & t = data.t;
			const auto & transform = data.transform;
			const auto & previousTransform = data.previousTransform;
			const auto & damping = conf.damping;
			const auto & windDamping = conf.windDamping;

#ifndef NDEBUG
			std::cout << "Solve Strand Dynamic" << std::endl;
#endif

			for (auto & strand : strands) {

				Eigen::VectorXf b(strand.getParticleSize() * 3), x(strand.getParticleSize() * 3);
				Eigen::MatrixXf A(strand.getParticleSize() * 3, strand.getParticleSize() * 3);
				A.setZero();

				const Eigen::Vector3f externalVelocityImpluse = t * conf.externalAccleration;

				for (size_t i = 0; i < strand.particlePtrs.size(); ++i) {
					const size_t i3 = i * 3;
					const auto & particlePtr = strand.particlePtrs[i];

					A.block<3, 3>(i3, i3) = (particlePtr->mass + windDamping * t) * Eigen::Matrix3f::Identity();
					b.segment<3>(i3) = particlePtr->mass * (strand.particlePtrs[i]->velocity + externalVelocityImpluse) + particleImpulses[particlePtr->globalIndex];
				}


				for (const auto & spring : strand.normalSprings) {
					spring.compute();
					const size_t i1 = spring.p1->localIndex;
					const size_t i2 = spring.p2->localIndex;

					const Eigen::Vector3f springImpluse = t * spring.f; //For p1

					b.segment<3>(i1 * 3) += springImpluse;
					b.segment<3>(i2 * 3) -= springImpluse;

					Eigen::Matrix3f vm = t * (spring.k * t + damping) * spring.dm; //Velocity matrix

					A.block<3, 3>(i1 * 3, i1 * 3) += vm;
					A.block<3, 3>(i1 * 3, i2 * 3) -= vm;
					A.block<3, 3>(i2 * 3, i2 * 3) += vm ;
					A.block<3, 3>(i2 * 3, i1 * 3) -= vm;
				}

				for (const auto & spring : strand.altitudeSprings) {

					spring.compute();

					const size_t i1 = spring.p1->localIndex;
					const size_t i2 = spring.p2->localIndex;
					const size_t i3 = spring.p3->localIndex;
					const size_t i4 = spring.p4->localIndex;

					const size_t i1_3 = i1 * 3;
					const size_t i2_3 = i2 * 3;
					const size_t i3_3 = i3 * 3;
					const size_t i4_3 = i4 * 3;

					const Eigen::Vector3f springImpluse = t * spring.f;

					b.segment<3>(i1_3) += springImpluse;
					switch (spring.selectionIndex) {
						case 0: //{p1,p2} -> {p3, p4}
							b.segment<3>(i2_3) += springImpluse;
							b.segment<3>(i3_3) -= springImpluse;
							b.segment<3>(i4_3) -= springImpluse;
							break;
						case 1: //{p1, p3} -> {p2, p4}
							b.segment<3>(i2_3) -= springImpluse;
							b.segment<3>(i3_3) += springImpluse;
							b.segment<3>(i4_3) -= springImpluse;
							break;
						case 2: //{p1, p4} -> {p2, p3}
							b.segment<3>(i2_3) -= springImpluse;
							b.segment<3>(i3_3) -= springImpluse;
							b.segment<3>(i4_3) += springImpluse;
							break;
						case 3: //{p1} -> {p2, p3, p4}
							b.segment<3>(i2_3) -= springImpluse;
							b.segment<3>(i3_3) -= springImpluse;
							b.segment<3>(i4_3) -= springImpluse;
							break;
						case 4: //{p2} -> {p1, p3, p4}
							b.segment<3>(i2_3) -= springImpluse;
							b.segment<3>(i3_3) += springImpluse;
							b.segment<3>(i4_3) += springImpluse;
							break;
						case 5: //{p3} -> {p1, p2, p4}
							b.segment<3>(i2_3) += springImpluse;
							b.segment<3>(i3_3) -= springImpluse;
							b.segment<3>(i4_3) += springImpluse;
							break;
						default: //{p4} -> {p1, p2, p3}
							b.segment<3>(i2_3) += springImpluse;
							b.segment<3>(i3_3) += springImpluse;
							b.segment<3>(i4_3) -= springImpluse;
							break;
					}
				}

				//Clear the first row and make the velocity match the body transform velocity
				for (size_t r = 0; r < 3; ++r)
					for (size_t c = 0; c < A.cols(); ++c)
						A(r, c) = 0.0f;
				A.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity();
				b.segment<3>(0) = (transform * strand.strandRootParticle->restPosition
				                   - previousTransform * strand.strandRootParticle->restPosition) / t;

				//Conjugate solver
				x = A.inverse() * b;

				//Assign back
				for (size_t i = 0; i < strand.particlePtrs.size(); ++i) {
					auto & particlePtr = strand.particlePtrs[i];
					particlePtr->velocity = x.segment<3>(i * 3);
					if (shouldUpdatePosition)
						particlePtr->position += t * particlePtr->velocity;
				}
			}
		}

		void solveStrainLimiting(const HairEngineSimulationData & data) {

			if (conf.enableStrainLimiting == false)
				return;

#ifndef NDEBUG
			std::cout << "Solve Strain Limiting" << std::endl;
#endif

			float tolerance = 1.0f + conf.strainLimitingLengthTolerance;
			float t_1 = 1.0f / data.t;

			for (auto & strand : strands) {
				for (auto & spring : strand.normalSprings)
					/*
					 * We only handle two situations:
					 *   1. Stretch Springs (which forms the segment of hair)
					 *   2. Virtual Stretch Spring that the second particle is virtual (which is the spring near to the hair root)
					 */
					if (spring.type == ParticleToParticleSpring::Stretch
					    || (spring.type == ParticleToParticleSpring::VirtualStretch && spring.p2->isVirtual())) {
						auto & pos1 = spring.p1->position;
						auto & pos2 = spring.p2->position;

						Eigen::Vector3f d = pos2 - pos1;
						float f = tolerance * spring.l0 / d.norm();
						if (f >= 1.0f)
							continue;

						//Fix the position to achieve that position next time
						spring.p2->position -= (1 - f) * d;
					}
			}
		}

		void solveObjectCollision(const HairEngineSimulationData & data, bool updateStrandRootPosition = false) {

#ifndef NDEBUG
			std::cout << "Solve Object Collision" << std::endl;
#endif

			if (!conf.enableObjectCollision || aggregate == nullptr) {
				//Directly update the position
				for (auto & particlePtr : particlePtrs)
					particlePtr->position += particlePtr->velocity * data.t;
				return;
			}

			const auto & f = conf.objectCollisionDegenerateFactor;
			const auto & t = data.t;
			const auto & miu = conf.objectCollisionFriction;

			const float diagnoalNorm = aggregate->worldDiagnoal().norm();
			const float isoContour = diagnoalNorm * conf.objectCollisionRelativeContourOffset;

			for (auto & particlePtr : particlePtrs) {
				//Only fix the normal particle's position
				if (particlePtr->isNormal() && (!particlePtr->isStrandRoot() || updateStrandRootPosition)) {
					Eigen::Vector3f outGradient;

					Eigen::Vector3f newPosition = particlePtr->position + particlePtr->velocity * data.t;
					float signedDistance = aggregate->distance(newPosition, &outGradient);

					//If it is outside the isoContour
					if (signedDistance > isoContour) {
						particlePtr->position = newPosition;
						continue;
					}

					outGradient.normalize();

					//Push the particle to the gradient
					//particlePtr->position += ((isoContour - signedDistance) * f) * outGradient;

					//Velocity for normal and tangent for the particles
					Eigen::Vector3f vpt, vpn;
					//Position for normal and tangent for the collision object
					Eigen::Vector3f v, vt, vn;
					Eigen::Vector3f & vp = particlePtr->velocity;

					v = aggregate->velocity(newPosition);
					EigenUtility::projection(vp, outGradient, vpn, vpt);
					EigenUtility::projection(v, outGradient, vn, vt);

					//Update the tangent velocity
					Eigen::Vector3f vrelt = vpt - vt;
					vrelt = std::max<float>(0.0f, 1.0f - miu * (vpn - vn).norm() / vrelt.norm()) * vrelt;
					vpt = vt + vrelt;

					//Update the normal velocity
					vpn = vn;

					particlePtr->velocity = vpt + vpn;
					particlePtr->position += particlePtr->velocity * data.t;

					//If the new position is in the inside the body, then push it again
					signedDistance = aggregate->distance(particlePtr->position, &outGradient);
					if (signedDistance <= isoContour)
						particlePtr->position += ((isoContour - signedDistance) * f) * outGradient.normalized();
				}
				else
					particlePtr->position += particlePtr->velocity * data.t; //Update the velocity
			}
		}

		struct SegmentComparatorStruct {
			float t1, t2;
			float distanceSquared;
			Segment::Ptr l2;
		};

		void solveHairContacts(const HairEngineSimulationData & data) {
			solveLineSegmentBasedHairContacts(data);
		}

		void solveLineSegmentBasedHairContacts(const HairEngineSimulationData & data) {

			/*
			 * 	Based on the strategy we use, a line segment has a continous record index, so that we could iterate one
			 * 	line segment with a index iterator. We use a heap to do this
			 */
			const static auto distanceComparator = [](const SegmentComparatorStruct & comparator1, const SegmentComparatorStruct & comparator2) -> bool {
				//We want to ensure that the segment are in order in test, to avoid equal distance for two segments
				// (always happen when for two continous segments in the same strand)
#ifndef HAIRENGINE_TEST
				return comparator1.distanceSquared < comparator2.distanceSquared;
#else
				return comparator1.distanceSquared < comparator2.distanceSquared
						|| (comparator1.distanceSquared == comparator2.distanceSquared && comparator1.l2->globalIndex < comparator2.l2->globalIndex);
#endif
			};

			std::fill(particleImpulses.begin(), particleImpulses.end(), Eigen::Vector3f::Zero());

			if (!conf.enableHairContacts && !conf.enableHairCollisions)
				return;

#ifndef NDEBUG
			std::cout << "Solve Line Segment Based Hair Contacts" << std::endl;
#endif

			/*
			 * Pre definition
			 */
			Eigen::Vector3f contactBounds = conf.hairContactsCreatingDistance * Eigen::Vector3f::Ones();

			const auto & creatingDistance = conf.hairContactsCreatingDistance;
			const auto & breakingDistance = conf.hairContactsBreakingDistance;
			const float creatingDistanceSquared = creatingDistance * creatingDistance;

			/*
			 * Grid insertion bounding box update
			 */
			Eigen::AlignedBox3f bound(normalParticlePtrs[0]->position);
			for (const auto & particlePtr : normalParticlePtrs)
				bound.extend(particlePtr->position);
			grid.refresh(bound);

			//offsets[7] used for hair contacts, offsetsc[7] used for hair collision
			std::vector<int> offsets[7], offsetsc[7];

			//Set offsets[7] if hair contacts is enabled
			if (conf.enableHairContacts) {
				Eigen::Vector3i w;
				for (size_t i = 0; i < 3; ++i) {
					w(i) = static_cast<int>(std::ceil(conf.hairContactsCreatingDistance * grid.getCellSizeInv()(i)));
				}

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1)
						for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1)
							offsets[0].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, iz)));

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1) {
						offsets[5].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, w.z())));
						offsets[6].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, -w.z())));
					}

				for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1)
					for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1) {
						offsets[1].push_back(grid.getIndex(Eigen::Vector3i(w.x(), iy, iz)));
						offsets[2].push_back(grid.getIndex(Eigen::Vector3i(-w.x(), iy, iz)));
					}

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1) {
						offsets[3].push_back(grid.getIndex(Eigen::Vector3i(ix, w.y(), iz)));
						offsets[4].push_back(grid.getIndex(Eigen::Vector3i(ix, -w.y(), iz)));
					}
			}

			if (conf.enableHairCollisions) {
				Eigen::Vector3i w;
				for (size_t i = 0; i < 3; ++i) {
					w(i) = static_cast<int>(std::ceil(conf.hairCollisionInterestingDistance * grid.getCellSizeInv()(i)));
				}

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1)
						for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1)
							offsetsc[0].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, iz)));

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1) {
						offsetsc[5].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, w.z())));
						offsetsc[6].push_back(grid.getIndex(Eigen::Vector3i(ix, iy, -w.z())));
					}

				for (int iy = 0; iy < w.y() + 1; iy = iy > 0 ? -iy : -iy + 1)
					for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1) {
						offsetsc[1].push_back(grid.getIndex(Eigen::Vector3i(w.x(), iy, iz)));
						offsetsc[2].push_back(grid.getIndex(Eigen::Vector3i(-w.x(), iy, iz)));
					}

				for (int ix = 0; ix < w.x() + 1; ix = ix > 0 ? -ix : -ix + 1)
					for (int iz = 0; iz < w.z() + 1; iz = iz > 0 ? -iz : -iz + 1) {
						offsetsc[3].push_back(grid.getIndex(Eigen::Vector3i(ix, w.y(), iz)));
						offsetsc[4].push_back(grid.getIndex(Eigen::Vector3i(ix, -w.y(), iz)));
					}
			}

			/*
			 * Compute the d and dInv for each segments
			 */
			for (auto & segmentPtr : segmentPtrs)
				segmentPtr->compute();

			std::vector<SegmentGridRecord> records;

			/*
			 * Grid insertion
			 */
			for (size_t i = 0; i < segmentPtrs.size(); ++i) {

				auto & segment = *segmentPtrs[i];

				const auto & d = segment.d;
				const auto & dInv = segment.dInv;
				const int moveX = (d.x() > 0.0f) ? 1 : -1;
				const int moveY = (d.y() > 0.0f) ? 1 : -1;
				const int moveZ = (d.z() > 0.0f) ? 1 : -1;

				SegmentGrid::GridIndex index = grid.getIndex(segment.p1->position);
				Eigen::AlignedBox3f initialBound = grid.getBound(index);

				grid.insert(&segment, index);
				records.emplace_back(&segment, index, 0);

				//Compute the next t for the bounding slab x, y and z
				float tx = (d.x() > 0.0f) ? segment.xSlabIntersection(initialBound.max().x())
				                          : (d.x() != 0.0f) ? segment.xSlabIntersection(initialBound.min().x())
				                                            : Constants::INF;
				float ty = (d.y() > 0.0f) ? segment.ySlabIntersection(initialBound.max().y())
				                          : (d.y() != 0.0f) ? segment.ySlabIntersection(initialBound.min().y())
				                                            : Constants::INF;
				float tz = (d.z() > 0.0f) ? segment.zSlabIntersection(initialBound.max().z())
				                          : (d.z() != 0.0f) ? segment.zSlabIntersection(initialBound.min().z())
				                                            : Constants::INF;

				//A cell's width (in x, y, z) in the segment's length measurement
				float dtx = std::abs(grid.getCellSize().x() * dInv.x());
				float dty = std::abs(grid.getCellSize().y() * dInv.y());
				float dtz = std::abs(grid.getCellSize().z() * dInv.z());

				float t;
				int minTIndex;
				int indicator;
				do {
					//Update t to the min{tx, ty, tz}
					minTIndex = 0; t = tx;
					if (t > ty) {
						minTIndex = 1; t = ty;
					}
					if (t > tz) {
						minTIndex = 2; t = tz;
					}

					if (t > 1.0f)
						break;

					switch (minTIndex) {
						case 0: //Move along x axis
							index += moveX > 0 ? grid.getOffsetX() : -grid.getOffsetX();
							indicator = moveX > 0 ? 1 : 2;
							tx += dtx;

							break;
						case 1:
							index += moveY > 0 ? grid.getOffsetY() : -grid.getOffsetY();
							indicator = moveY > 0 ? 3 : 4;
							ty += dty;

							break;
						case 2:
							index += moveZ > 0 ? grid.getOffsetZ() : -grid.getOffsetZ();
							indicator = moveZ > 0 ? 5 : 6;
							tz += dtz;

							break;
					}

					//Insert it to the new grid
					grid.insert(&segment, index);
					records.emplace_back(&segment, index, indicator);

				} while (true);
			}

			/*
			 * Solve hair contacts
			 */
			if (conf.enableHairContacts) {
				/*
				 * For each segment pairs (currently exisits), use "compute()" to get the current length.
				 * Clear all the segment with distance larger than the breaking distance
				 */
				for (auto & sp : segmentPtrs) {

					auto deletedEnd = sp->contactSpringBegin();
					for (auto l = sp->contactSpringBegin(); l < sp->contactSpringEnd(); ++l) {
						l->compute();

						if (l->l >= breakingDistance) {
							--l->l1->nUndirectedContacts;
							--l->l2->nUndirectedContacts;
							std::allocator<HairContactSpring>().destroy(l);
						}
						else {
							if (deletedEnd != l)
								*deletedEnd = *l;
							++deletedEnd;
						}
					}

					//We move this out of the loop so that the sp->contactSpringEnd() remain unchanged
					sp->nContacts = deletedEnd - sp->contactSpringBegin();
				}

				std::fill(segmentCheck.begin(), segmentCheck.end(), nullptr);

				for (size_t recordBeginIndex = 0, recordEndIndex = 1; recordBeginIndex < records.size(); recordBeginIndex = recordEndIndex) {
					while (recordEndIndex < records.size() && records[recordEndIndex].l == records[recordBeginIndex].l)
						++recordEndIndex;

					auto & l1 = records[recordBeginIndex].l;

					if (l1->nUndirectedContacts >= conf.hairContactsMaxContactPerSegment)
						continue;

					const size_t nNeeds = conf.hairContactsMaxContactPerSegment - l1->nUndirectedContacts;

					//Fill the contactSpringCheck for the corresponding segment
					for (auto l = l1->contactSpringBegin(); l < l1->contactSpringEnd(); ++l)
						segmentCheck[l->l2->globalIndex] = l->l1;

					std::priority_queue<SegmentComparatorStruct, std::vector<SegmentComparatorStruct>, decltype(distanceComparator)> heap(distanceComparator);

					//Iterate over recordBeginIndex to recordEndIndex, find all the "potential connected" segment pointer
					for (size_t i = recordBeginIndex; i < recordEndIndex; ++i) {
						const auto & record = records[i];
						for (int offset : offsets[record.indicator]) {
							const auto & ls = grid.getElementsInCell(record.index + offset); //An invalid index will yield empty element list
							for (auto l2 : ls) {

								if (!(l2->globalIndex > l1->globalIndex &&
								      l2->nUndirectedContacts < conf.hairContactsMaxContactPerSegment &&
								      segmentCheck[l2->globalIndex] != l1 &&
								      l2->strandIndex() != l1->strandIndex()))
										continue;

									segmentCheck[l2->globalIndex] = l1;

									SegmentComparatorStruct comparator;
									comparator.l2 = l2;
									comparator.distanceSquared = MathUtility::lineSegmentSquaredDistance(l1->p1->position, l1->p2->position, l2->p1->position, l2->p2->position, comparator.t1, comparator.t2);

									if (comparator.distanceSquared < creatingDistanceSquared) {
										heap.push(comparator);
										if (heap.size() > nNeeds)
											heap.pop();
									}
							}
						}
					}

					while (!heap.empty()) {
						const auto & comparator = heap.top();

						std::allocator<HairContactSpring>().construct(l1->contactSpringEnd(), l1, comparator.l2, comparator.t1, comparator.t2, std::sqrt(comparator.distanceSquared), conf.contactStiffness);
						l1->contactSpringEnd()->compute();

						++l1->nContacts;
						++l1->nUndirectedContacts;
						++comparator.l2->nUndirectedContacts;

						heap.pop();
					}
				}

				for (auto & segmentPtr : segmentPtrs) {
					for (auto l = segmentPtr->contactSpringBegin(); l != segmentPtr->contactSpringEnd(); ++l) {

						l->compute();

						Eigen::Vector3f impluse = data.t * l->f;
						particleImpulses[l->l1->p1->globalIndex] += impluse * (1 - l->t1);
						particleImpulses[l->l1->p2->globalIndex] += impluse * l->t1;
						particleImpulses[l->l2->p1->globalIndex] -= impluse * (1 - l->t2);
						particleImpulses[l->l2->p2->globalIndex] -= impluse * l->t2;
					}
				}
			} //End solve hair contacts

			if (conf.enableHairCollisions) {

//				std::fill(segmentCheck.begin(), segmentCheck.end(), nullptr);

//				for (size_t recordBeginIndex = 0, recordEndIndex = 1; recordBeginIndex < records.size(); recordBeginIndex = recordEndIndex) {
//
//					while (recordEndIndex < records.size() && records[recordEndIndex].l == records[recordBeginIndex].l)
//						++recordEndIndex;
//
//					auto & l1 = records[recordBeginIndex].l;
//
//					for (size_t i = recordBeginIndex; i < recordEndIndex; ++i) {
//						const auto & record = records[i];
//						for (int offset : offsetsc[record.indicator]) {
//
//							const auto & ls = grid.getElementsInCell(record.index + offset);
//							for (auto l2 : ls) {
//
//								if (!(l2->globalIndex > l1->globalIndex &&
//								      segmentCheck[l2->globalIndex] != l1 &&
//								      l2->strandIndex() != l1->strandIndex()))
//									continue;
//
//								segmentCheck[l2->globalIndex] = l1;
//
//								float t1, t2;
//
//								MathUtility::lineSegmentSquaredDistance(l1->p1->position, l1->p2->position, l2->p1->position, l2->p2->position, t1, t2);
//
//								//Check whether they have a actual collision
//								Eigen::Vector3f dNow = MathUtility::lerp(l2->p1->position, l2->p2->position, t2) - MathUtility::lerp(l1->p1->position, l1->p2->position, t1);
//
//								Eigen::Vector3f l1p1Predict = l1->p1->position + l1->p1->velocity * data.t;
//								Eigen::Vector3f l1p2Predict = l1->p2->position + l1->p2->velocity * data.t;
//								Eigen::Vector3f l2p1Predict = l2->p1->position + l2->p1->velocity * data.t;
//								Eigen::Vector3f l2p2Predict = l2->p2->position + l2->p2->velocity * data.t;
//
//								float t1Predict, t2Predict;
//
//								MathUtility::lineSegmentSquaredDistance(l1p1Predict, l1p2Predict, l2p1Predict, l2p2Predict, t1Predict, t2Predict);
//
//								Eigen::Vector3f dPredict = MathUtility::lerp(l2p1Predict, l2p2Predict, t2Predict) - MathUtility::lerp(l1p1Predict, l1p2Predict, t1Predict);
//
//								//Acutal collision happend
//								if (dNow.dot(dPredict) < 0.0f) {
//									//Fix the velocity
//									Eigen::Vector3f averageVelocity = (l1->p1->velocity + l1->p2->velocity + l2->p1->velocity + l2->p2->velocity) / 4.0f;
//									l1->p1->velocity = l1->p2->velocity = l2->p1->velocity = l2->p2->velocity = averageVelocity;
//								}
//
//							} //End l2 iteration
//						} //End offset iteration
//					} //End record iteration
//				}

				auto deletedEnd = std::remove_if(collisionInfos.begin(), collisionInfos.end(), [&data]( const CollisionInfo & info) -> bool {
					Eigen::Vector3f d = info.estimatedD(data.t);
					return (d.dot(info.n) > 0.0f && d.norm() > info.l0);
				});

				collisionInfos.erase(deletedEnd, collisionInfos.end());

				std::unordered_set<std::pair<Segment::Ptr, Segment::Ptr>, CollisionInfo::Hasher> collisionPairs;
				for (const auto & info : collisionInfos)
					collisionPairs.insert(std::make_pair(info.s1, info.s2));

				for (size_t i = 0; i < segmentPtrs.size(); ++i) {

					auto s1 = segmentPtrs[i];

					for (size_t j = i + 1; j < segmentPtrs.size(); ++j) {
						auto s2 = segmentPtrs[j];

						if (s1->strandIndex() == s2->strandIndex() || collisionPairs.find(std::make_pair(s1, s2)) != collisionPairs.end())
							continue;

						float t1, t2;
						MathUtility::lineSegmentSquaredDistance(s1->p1->position, s1->p2->position, s2->p1->position, s2->p2->position, t1, t2);

						if (t1 <= 0.0f || t1 >= 1.0f || t2 <= 0.0f || t2 >= 1.0f)
							continue;

						Eigen::Vector3f s2p = MathUtility::lerp(s2->p1->position, s2->p2->position, t2);
						Eigen::Vector3f s1p = MathUtility::lerp(s1->p1->position, s1->p2->position, t1);

						Eigen::Vector3f d = s2p - s1p;
						Eigen::Vector3f n = d.normalized();

						Eigen::Vector3f s2nv = MathUtility::lerp(s2->p1->velocity, s2->p2->velocity, t2);
						s2nv = MathUtility::project(s2nv, n);
						Eigen::Vector3f s1nv = MathUtility::lerp(s1->p1->velocity, s1->p2->velocity, t1);
						s1nv = MathUtility::project(s1nv, n);

						Eigen::Vector3f dnv = s2nv - s1nv;

						if (dnv.dot(d) >= 0.0f || dnv.norm() * data.t < d.norm())
							continue;

						float time = d.norm() / (dnv.norm() * data.t);

						if (time >= 1.0f)
							continue;

						//Collision happen and fixed
						collisionInfos.emplace_back(s1, s2, t1, t2, n, d.norm());
					}
				}

				struct ParticleCollisionImpulse {
					Eigen::Vector3f impulse;
					int count;

					void addImpulse(const Eigen::Vector3f & im) {
						impulse += im;
						++count;
					}

					Eigen::Vector3f averageImpulse(int impulseMaxCount = 1) const {
						if (count == 0)
							return Eigen::Vector3f::Zero();

						return impulse * (std::min<int>(impulseMaxCount, count) / (float)count);
					}

					ParticleCollisionImpulse():
							impulse(Eigen::Vector3f::Zero()), count(0) {}
				};

				std::vector<ParticleCollisionImpulse> particleCollisionImpulses(particlePtrs.size());

				for (auto & info : collisionInfos) {
					Eigen::Vector3f d = info.d();

					constexpr const float k = 25000.0f;

					float sign = d.dot(info.n) > 0.0f ? 1.0f : -1.0f;
					float l = sign * d.norm() - info.l0;
					Eigen::Vector3f impulse = (k * l * data.t) * (sign * d.normalized());

					particleCollisionImpulses[info.s1->p1->globalIndex].addImpulse(impulse * (1 - info.t1));
					particleCollisionImpulses[info.s1->p2->globalIndex].addImpulse(impulse * info.t1);
					particleCollisionImpulses[info.s2->p1->globalIndex].addImpulse(impulse * (info.t2 - 1));
					particleCollisionImpulses[info.s2->p2->globalIndex].addImpulse(-impulse * info.t2);
				}

				for (size_t i = 0; i < particlePtrs.size(); ++i) {
					particleImpulses[i] += particleCollisionImpulses[i].averageImpulse(3);
				}
			}
		}
	};
}
